from DebugTrace import DebugTrace
from Drawing import Drawing


#Current assumption in the homing algorithm is to alwasy home to the
#min endstop...  The homing options should be taken from the config file

class MotorHandler:
    def __init__(self,debugLevel=1):
        self.distanceUnits="mm"
        self.positioningMode="absolute"
        self.extruderMode="relative"
        self.trace = DebugTrace("MotorHandler",debugLevel)
        self.mc=None #Motor Controller

    def move(self,x,y,z,e):
        self.trace.trace(2,"Moving to X:"+str(x)+" Y:"+str(y)+" Z:"+str(z)+" E:"+str(e))

        xDist=None
        yDist=None
        zDist=None
        eDist=None
        
        if (self.positioningMode == "absolute"):
            xDist=self.mc.getDistanceToPosition("X",x)
            yDist=self.mc.getDistanceToPosition("Y",y)
            zDist=self.mc.getDistanceToPosition("Z",z)
        elif (self.positioningMode == "relative"):
            xDist=x
            yDist=y
            zDist=z
        else:
            self.trace.trace(1,"Positioning mode is invalid!!! Value="+self.positioningMode)
            return "!!"

        eMotor=self.mc.getCurrentToolMotor()
        if(eMotor==None):
            self.trace.trace(1,"No motor found for current tool!")
            return "!!"
                    
              
        if (self.extruderMode == "absolute"):
            eDist=self.mc.getDistanceToPosition(eMotor,e)
        elif(self.extruderMode == "relative"):
            eDist=e
        else:
              self.trace.trace(1,"Extruder mode is invalid!!! Value="+self.extruderMode)
              return "!!"

        self.mc.move(xDist,yDist,zDist,eDist)

        return "ok"

    def home(self,x,y,z,e):
        xHome = None
        yHome = None
        zHome = None
        eHome = None
        eMotor = self.mc.getCurrentToolMotor()

        self.trace.trace(2,"Homing")
        
        if (self.positioningMode == "absolute"):
            if (x!=None):
                xHome=self.mc.motors["X"].minEndStop-5
            if (y!=None):
                yHome=self.mc.motors["Y"].minEndStop-5
            if (z!=None):
                zHome=self.mc.motors["Z"].minEndStop-5
            if (e!=None):
                if (self.extruderMode == "absolute"):
                    eHome=0
                elif (self.extruderMode == "relative"):
                    eHome=eMotor.minEndStop-5
                else:
                    self.trace.trace(1,"Extruder mode is invalid!!! Value="+self.extruderMode)
                    return "!!"
                
            if (x==None and y==None and z==None and e==None):
                xHome=0
                yHome=0
                zHome=0
                if (self.extruderMode == "absolute"):
                    eHome=0
                elif (self.extruderMode == "relative"):
                    eHome=0
                else:
                    self.trace.trace(1,"Extruder mode is invalid!!! Value="+self.extruderMode)
                    return "!!"
                            
        elif (self.positioningMode == "relative"):  #Currently assuming we always home to min endstops
            if (x!=None):
                xHome=self.mc.motors["X"].getDistanceToEndstop("min") - 5 #Add a bit so it physically hits the endstop
            if (y!=None):
                yHome=self.mc.motors["Y"].getDistanceToEndstop("min") - 5
            if (z!=None):
                zHome=self.mc.motors["Z"].getDistanceToEndstop("min") - 5
            if (e!=None):
                if (self.extruderMode == "absolute"):
                    eHome=0
                elif (self.extruderMode == "relative"):
                    eHome=eMotor.minEndStop-5
                else:
                    self.trace.trace(1,"Extruder mode is invalid!!! Value="+self.extruderMode)
                    return "!!"
                
            if (x==None and y==None and z==None and e==None):
                xHome=self.mc.motors["X"].getDistanceToEndstop("min") - 5
                yHome=self.mc.motors["Y"].getDistanceToEndstop("min") - 5
                zHome=self.mc.motors["Z"].getDistanceToEndstop("min") - 5

                if (self.extruderMode == "absolute"):
                    eHome=0
                elif (self.extruderMode == "relative"):
                    eHome=eMotor.minEndStop-5
                else:
                    self.trace.trace(1,"Extruder mode is invalid!!! Value="+self.extruderMode)
                    return "!!"
        else:
            self.trace.trace(1,"Positioning mode is invalid!!! Value="+self.positioningMode)
            return "!!"
        
        return self.move(xHome,yHome,zHome,eHome)

    def setMovementUnits(self,unit):
        if(unit=="mm" or unit=="inch"):
            self.distanceUnits=unit
            self.trace.trace(2,"Setting movement unit to "+unit)
            return "ok"
        else:
            self.trace.trace(1,"Invalid movement unit "+unit)
            return "!!"

    def setPositioningMode(self,mode):
        if (mode=="relative" or mode=="absolute"):
            self.positioningMode=mode
            self.trace.trace(2,"Setting positioning mode to "+mode)
            return "ok"
        else:
            self.trace.trace(1,"Invalid positioning mode "+mode)
            return "!!"

    def setExtruderMode(self,mode):
        if (mode=="relative" or mode=="absolute"):
            self.extruderMode=mode
            self.trace.trace(2,"Setting extruder mode to "+mode)
            return "ok"
        else:
            self.trace.trace(1,"Invalid extruder mode "+mode)
            return "!!"

    def setTool(self,tool):
        return self.mc.setTool(tool)
        


    def setPosition(self,x,y,z,e):
        if (x!=None):
            self.mc.setPosition("X",x)
            self.trace.trace(2,"Setting current X position to "+str(x))
        if (y!=None):
            self.mc.setPosition("Y",y)
            self.trace.trace(2,"Setting current Y position to "+str(y))
        if (z!=None):
            self.mc.setPosition("Z",z)
            self.trace.trace(2,"Setting current Z position to "+str(z))
        if (e!=None):
            self.mc.setPosition("E0",e)
            self.trace.trace(2,"Setting current E position to "+str(e))
        return "ok"

    def holdMotors(self):
        self.trace.trace(2,"Holding motors")
        self.mc.holdMotors()
        return "ok" #Don't actually know how this will work yet

    def releaseMotors(self):
        self.trace.trace(2,"Releasing motors")
        self.mc.releaseMotors()
        return "ok" #Don't know how this will work yet
    
